
#ifndef __COMM_COMB_MAP_CPP__
#define __COMM_COMB_MAP_CPP__

#include "comm/comm_comb_map.h"
#include "comm/comm_map.h"
#include "comm/lib/tinyxml.h"
#include <iostream>
#include <list>
#include <vector>

using namespace std;

/**************************************************************************************
功能描述: 从XML文件读组合地图信息
修改记录:
**************************************************************************************/
int COMB_MAP_FILE_C::Read(list<SUB_MAP_COMB_INFO_STRU> &lstSubMap)
{
    TiXmlDocument objXmlFile(strFilePath_.c_str());
    if (!objXmlFile.LoadFile()) {
        ST_LOG_ERR("Load File(%s) Fail.", strFilePath_.c_str());
        return -1;
    }

    TiXmlElement *pobjCombMap = objXmlFile.RootElement();
    if (pobjCombMap == NULL) {
        return -1;
    }

    for (TiXmlElement *pobjSubMap = pobjCombMap->FirstChildElement();
         pobjSubMap != NULL;
         pobjSubMap = pobjSubMap->NextSiblingElement()) {
        SUB_MAP_COMB_INFO_STRU stSubMapInfo;
        if (TIXML_SUCCESS != pobjSubMap->QueryUnsignedAttribute("No",         &stSubMapInfo.mapNo)) {
            return -1;
        }

        if (TIXML_SUCCESS != pobjSubMap->QueryFloatAttribute(   "ZeroPoseX",  &stSubMapInfo.stZeroPoseByCombMap.stPos.x)) {
            return -1;
        }

        if (TIXML_SUCCESS != pobjSubMap->QueryFloatAttribute(   "ZeroPoseY",  &stSubMapInfo.stZeroPoseByCombMap.stPos.y)) {
            return -1;
        }
        
        if (TIXML_SUCCESS != pobjSubMap->QueryFloatAttribute(   "ZeroPoseA",  &stSubMapInfo.stZeroPoseByCombMap.dirAngle)) {
            return -1;
        }

        lstSubMap.push_back(stSubMapInfo);
    }

    return 0;
}

/**************************************************************************************
功能描述: 组合地图信息写XML文件
修改记录:
**************************************************************************************/
int COMB_MAP_FILE_C::Write(list<SUB_MAP_COMB_INFO_STRU> &lstSubMap)
{
    /* 该类的析构函数中会释放所有子节点，无需额外释放 */
    TiXmlDocument objXmlFile;

    TiXmlDeclaration *pobjDeclare = new TiXmlDeclaration("1.0", "UTF-8", "");
    if (pobjDeclare == NULL) {
        return -1;
    }
    objXmlFile.LinkEndChild(pobjDeclare);

    /* 根节点 */
    TiXmlElement *pobjCombMap = new TiXmlElement("CombMap");
    if (pobjCombMap == NULL) {
        return -1;
    }
    objXmlFile.LinkEndChild(pobjCombMap);

    /* 子地图节点 */
    for (list<SUB_MAP_COMB_INFO_STRU>::iterator iter = lstSubMap.begin();
         iter != lstSubMap.end();
         ++iter) {
        TiXmlElement *pobjSubMap = new TiXmlElement("SubMap");
        if (pobjSubMap == NULL) {
            return -1;
        }
        
        pobjSubMap->SetAttribute(      "No",        iter->mapNo);
        pobjSubMap->SetDoubleAttribute("ZeroPoseX", iter->stZeroPoseByCombMap.stPos.x);
        pobjSubMap->SetDoubleAttribute("ZeroPoseY", iter->stZeroPoseByCombMap.stPos.y);
        pobjSubMap->SetDoubleAttribute("ZeroPoseA", iter->stZeroPoseByCombMap.dirAngle);

        pobjCombMap->LinkEndChild(pobjSubMap);
    }

    objXmlFile.SaveFile(strFilePath_.c_str());
    return 0;
}


SUB_MAP_C::SUB_MAP_C(UINT32 mapNo, IMAGE_COORDINATE_STRU &stMapSize, COORDINATE_2D_STRU &stOriginPosBySubMap, COOR_2D_POSE_STRU &stZeroPoseByCombMap)
{
    no_ = mapNo;
    
    stMapSize_.x = stMapSize.x;
    stMapSize_.y = stMapSize.y;

    stOriginPosBySubMap_.Copy(stOriginPosBySubMap);
    
    stZeroPoseByCombMap_.Copy(stZeroPoseByCombMap);

    objCombToSubMapTf_.SetTransRelationship(stZeroPoseByCombMap);

    GetMapAreaByCombMap(objSubMapAreaByCombMap_);
}

UINT32 SUB_MAP_C::GetMapNo(void)
{
    return no_;
}

void SUB_MAP_C::GetOriginPoseByCombMap(COOR_2D_POSE_STRU &stOriginPoseByCombMap)
{
    COOR_2D_POSE_STRU stOriginPoseBySubMap;
    stOriginPoseBySubMap.stPos.Copy(stOriginPosBySubMap_);
    stOriginPoseBySubMap.dirAngle = 0.0;

    objCombToSubMapTf_.TransSonPoseToFather(stOriginPoseBySubMap, stOriginPoseByCombMap);
}

void SUB_MAP_C::GetInfo(UINT32 &mapNo, COOR_2D_POSE_STRU &stZeroPoseByCombMap)
{
    mapNo = no_;
    stZeroPoseByCombMap.Copy(stZeroPoseByCombMap_);
}

/**************************************************************************************
功能描述: 将子地图的位置转换为组合地图上的位置
修改记录:
**************************************************************************************/
void SUB_MAP_C::TransSubToCombMapPose(COOR_2D_POSE_STRU &stSubMapPose, COOR_2D_POSE_STRU &stCombMapPose)
{
    objCombToSubMapTf_.TransSonPoseToFather(stSubMapPose, stCombMapPose);
}

/**************************************************************************************
功能描述: 将组合地图的位置转换为子地图上的位置
修改记录:
**************************************************************************************/
void SUB_MAP_C::TransCombToSubMapPose(COOR_2D_POSE_STRU &stCombMapPose, COOR_2D_POSE_STRU &stSubMapPose)
{
    objCombToSubMapTf_.TransFatherPoseToSon(stCombMapPose, stSubMapPose);
}

/**************************************************************************************
功能描述: 判断指定点是否在当前地图范围内
修改记录:
**************************************************************************************/
bool SUB_MAP_C::IsPointInMap(const COORDINATE_2D_STRU &stPosByCombMap)
{
    return objSubMapAreaByCombMap_.IsPointIn(stPosByCombMap);
}

/**************************************************************************************
功能描述: 获取指定点到地图边缘最小距离
          调用方保证给定点在地图范围内
修改记录:
**************************************************************************************/
float SUB_MAP_C::GetMinDistSquToMapEdge(const COORDINATE_2D_STRU &stPosByCombMap)
{
    return objSubMapAreaByCombMap_.GetMinDistSquOfInternalPointToEdge(stPosByCombMap);
}

/**************************************************************************************
功能描述: 获取沿前进方向到地图边缘的距离
          调用方保证给定点在地图范围内
修改记录:
**************************************************************************************/
float SUB_MAP_C::GetDistSquToMapEdgeAlongMoveDir(const COOR_2D_POSE_STRU &stCurCarPose)
{
    return objSubMapAreaByCombMap_.GetDistSquOfInternalPointAlongDirToEdge(stCurCarPose.stPos, stCurCarPose.dirAngle);
}

/**************************************************************************************
功能描述: 获取本子地图对应的组合地图上的多边形区域
修改记录:
**************************************************************************************/
void SUB_MAP_C::GetMapAreaByCombMap(CONVEX_POLYGON_C &objArea)
{
    /* 从原定开始逆时针4个顶点 */
    COORDINATE_2D_STRU astMapVerticeBySubMap[] = {{stOriginPosBySubMap_.x,                                               stOriginPosBySubMap_.y},
                                                  {stOriginPosBySubMap_.x + (float)stMapSize_.x * float(MAP_RESOLUTION), stOriginPosBySubMap_.y},
                                                  {stOriginPosBySubMap_.x + (float)stMapSize_.x * float(MAP_RESOLUTION), stOriginPosBySubMap_.y + (float)stMapSize_.y * float(MAP_RESOLUTION)},
                                                  {stOriginPosBySubMap_.x,                                               stOriginPosBySubMap_.y + (float)stMapSize_.y * float(MAP_RESOLUTION)}};
    
    COORDINATE_2D_STRU astMapVerticeByCombMap[ARRAY_SIZE(astMapVerticeBySubMap)];

    for (UINT32 loop = 0; loop < ARRAY_SIZE(astMapVerticeBySubMap); loop++) {
        objCombToSubMapTf_.TransSonPosToFather(astMapVerticeBySubMap[loop], astMapVerticeByCombMap[loop]);
    }

    objArea.Reset(astMapVerticeByCombMap, ARRAY_SIZE(astMapVerticeByCombMap));
}

INT32 COMB_MAP_C::Init(void)
{
    /* 从文件加载地图信息 */
    INT32 rslt = LoadFromFile();
    if (rslt != 0) {
        ST_LOG_ERR("LoadFromFile() Fail.");
        return -1;
    }

    ST_LOG_INFO("Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 创建新的子地图
          用于建图模式下
修改记录:
**************************************************************************************/
int COMB_MAP_C::CreateSubMap(COOR_2D_POSE_STRU &stSubMapZeroPoseByCombMap, string &strSubMapName)
{
    IMAGE_COORDINATE_STRU stMapSize = {100, 100};               /* 建图场景建立新子地图时，不需要指定地图大小 */
    COORDINATE_2D_STRU    stSubMapOriginPos = {0.0, 0.0};       /* 建图场景建立新子地图时，不需要指定地图原点信息 */    
    if (0 != AddSubMap(minValidSubMapNo_, stMapSize, stSubMapOriginPos, stSubMapZeroPoseByCombMap)) {
        ST_LOG_ERR("Add Sub Map(%u) Fail.", minValidSubMapNo_);
        return -1;
    }

    if (0 != ChangeSubMapByNo(minValidSubMapNo_)) {
        ST_LOG_ERR("Change Map(%u) Fail.", minValidSubMapNo_);
        return -1;
    }

    GetSubMapName(minValidSubMapNo_, strSubMapName);
    ST_LOG_INFO("Succ. No=%u, Name=%s.", minValidSubMapNo_, strSubMapName.c_str());
    
    minValidSubMapNo_++;
    /* 本地信息变更后需要保存到文件中 */
    SaveToFile();
    return 0;
}

/**************************************************************************************
功能描述: 判断是否需要更换子地图，如果需要返回子地图名称
修改记录:
**************************************************************************************/
bool COMB_MAP_C::IsChangeSubMap(const COOR_2D_POSE_STRU &stCurCarPose, string &strSubMapName, COOR_2D_POSE_STRU &stNewMapSetPose)
{
    stNewMapSetPose.Copy(stCurCarPose);
    
    if ((pobjWorkingSubMap_ == NULL) || (!pobjWorkingSubMap_->IsPointInMap(stCurCarPose.stPos))) {
        if (pobjWorkingSubMap_ == NULL) {
            ST_LOG_INFO("No Pre Sub Map.");
        } else {
            ST_LOG_ERR("Now Not In Any Map. x = %f , y = %f.", stCurCarPose.stPos.x, stCurCarPose.stPos.y);
        }

        /* 如果之前没有加载子地图，或者当前位置已经不在工作地图中了，则直接获取最匹配的子地图 */
        SUB_MAP_C *pobjMatchSubMap = MatchSubMapByUsingMinDist(stCurCarPose, -1);
        if (pobjMatchSubMap == NULL) {
            ST_LOG_ERR("Can Not Match Sub Map.x=%f, y=%f. And To Use 1st Map.", stCurCarPose.stPos.x, stCurCarPose.stPos.y);

            /* 如果找不到匹配的地图，则直接使用第一个地图，初始化位置设置为该地图原点 */
            if (lstSubMap_.size() == 0) {
                ST_LOG_ERR("No Sub Map.");
                return false;
            } else {
                pobjMatchSubMap = lstSubMap_.front();
                pobjMatchSubMap->GetOriginPoseByCombMap(stNewMapSetPose);
            }
        }

        GetSubMapName(pobjMatchSubMap->GetMapNo(), strSubMapName);
        return true;
    } else {
        /* 当前已经有运行的子地图，前进方向靠近地图边缘小于一定距离(50m)后，才开始尝试查找更匹配的地图 */
        float distSquToCurSubMapEdgeAlongMoveDir = pobjWorkingSubMap_->GetDistSquToMapEdgeAlongMoveDir(stCurCarPose);        
        if ((distSquToCurSubMapEdgeAlongMoveDir > 2500.0) || (distSquToCurSubMapEdgeAlongMoveDir < 0.0)) {
            //ST_LOG_ERR("DistSqu(%f) Not Need To Change.", distSquToCurSubMapEdgeAlongMoveDir);
            return false;
        }

        /* 按照前进的反方向，匹配该方向上最大距离的子地图 */
        COOR_2D_POSE_STRU stCurMovePoseReDir;
        stCurMovePoseReDir.stPos.Copy(stCurCarPose.stPos);
        stCurMovePoseReDir.dirAngle = AdjustAngle(stCurCarPose.dirAngle + PI);
        float distSquToMatchSubMapEdgeAlongReDir;
        SUB_MAP_C *pobjMatchSubMap = MatchSubMapByUsingMoveDirDist(stCurMovePoseReDir, pobjWorkingSubMap_->GetMapNo(), distSquToMatchSubMapEdgeAlongReDir);
        if ((pobjMatchSubMap == NULL) || (distSquToMatchSubMapEdgeAlongReDir < 0.0)) {
            /* 如果很靠边了还无法匹配新的子地图则记录日志，可能误记录 */
            /*
            if (distSquToCurSubMapEdge < 400.0) {
                ST_LOG_ERR("Near(Squ=%f) Map(%u), But Can Not Match New Sub Map.x=%f, y=%f.", distSquToCurSubMapEdge, pobjWorkingSubMap_->GetMapNo(), stCurCarPose.stPos.x, stCurCarPose.stPos.y);
            }
            */
            //ST_LOG_ERR("Can Not Match New.");
            return false;
        }

        /* 匹配子地图中背后距边距离时当前地图中前方距边距离的2倍时，才启动切换地图 */
        bool isChange = distSquToMatchSubMapEdgeAlongReDir > (4.0 * distSquToCurSubMapEdgeAlongMoveDir);
        if (isChange) {
            GetSubMapName(pobjMatchSubMap->GetMapNo(), strSubMapName);
        }
        //ST_LOG_ERR("NewDistSqu=%f, OldDistSqu=%f, IsChange=%d, MapName=%s.", distSquToMatchSubMapEdgeAlongReDir, distSquToCurSubMapEdgeAlongMoveDir, isChange, strSubMapName.c_str());
        return isChange;
    }
}

/**************************************************************************************
功能描述: 切换工作子地图
修改记录:
**************************************************************************************/
int COMB_MAP_C::ChangeSubMap(string &strSubMapName)
{
    int subMapNo = objMapFileMng_.GetSubMapNo(strSubMapName);
    if (subMapNo < 0) {
        ST_LOG_ERR("Name(%s) To No Fail.", strSubMapName.c_str());
        return -1;
    }

    if (0 != ChangeSubMapByNo((UINT32)subMapNo)) {
        ST_LOG_ERR("Change SubMap(%d) Fail.", subMapNo);
        return -1;
    }

    ST_LOG_INFO("Succ. No=%d, Name=%s.", subMapNo, strSubMapName.c_str());
    return 0;
}

/**************************************************************************************
功能描述: 将子地图的位姿转换为组合地图的位姿
修改记录:
**************************************************************************************/
int COMB_MAP_C::TransSubToCombMapPose(COOR_2D_POSE_STRU &stSubMapPose, COOR_2D_POSE_STRU &stCombMapPose)
{
    if (pobjWorkingSubMap_ == NULL) {
        return -1;
    }

    pobjWorkingSubMap_->TransSubToCombMapPose(stSubMapPose, stCombMapPose);
    return 0;
}

/**************************************************************************************
功能描述: 将组合地图的位姿转换为子地图的位姿
修改记录:
**************************************************************************************/
int COMB_MAP_C::TransCombToSubMapPose(COOR_2D_POSE_STRU &stCombMapPose, COOR_2D_POSE_STRU &stSubMapPose)
{
    if (pobjWorkingSubMap_ == NULL) {
        return -1;
    }

    pobjWorkingSubMap_->TransCombToSubMapPose(stCombMapPose, stSubMapPose);
    return 0;
}

/**************************************************************************************
功能描述: 获取指定编号地图的名称
修改记录:
**************************************************************************************/
void COMB_MAP_C::GetSubMapName(UINT32 subMapNo, string &strMapName)
{
    objMapFileMng_.GetSubMapName(subMapNo, strMapName);
}

/**************************************************************************************
功能描述: 获取子地图号列表
修改记录:
**************************************************************************************/
void COMB_MAP_C::GetSubMapList(list<UINT32> &lstSubMapNo)
{
    lstSubMapNo.clear();
    
    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        SUB_MAP_C *pstSubMap = *iter;
        UINT32 subMapNo = pstSubMap->GetMapNo();
        lstSubMapNo.push_back(subMapNo);
    }
}

/**************************************************************************************
功能描述: 获取当前工作的子地图号
修改记录:
**************************************************************************************/
int COMB_MAP_C::GetWorkSubMapNo(void)
{
    /* 不会为空，仅为保护 */
    if (pobjWorkingSubMap_ == NULL) {
        return -1;
    }

    return (int)pobjWorkingSubMap_->GetMapNo();
}

int COMB_MAP_C::AddSubMap(UINT32 mapNo, IMAGE_COORDINATE_STRU &stMapSize, COORDINATE_2D_STRU &stOriginPos, COOR_2D_POSE_STRU &stZeroPoseByCombMap)
{
    SUB_MAP_C *pobjSubMap = new SUB_MAP_C(mapNo, stMapSize, stOriginPos, stZeroPoseByCombMap);
    if (pobjSubMap == NULL) {
        ST_LOG_ERR("New Obj Fail.");
        return -1;
    }

    lstSubMap_.push_back(pobjSubMap);
    return 0;
}

void COMB_MAP_C::Clear(void)
{
    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        delete (*iter);
    }

    lstSubMap_.clear();
}

/**************************************************************************************
功能描述: 切换工作子地图
修改记录:
**************************************************************************************/
int COMB_MAP_C::ChangeSubMapByNo(UINT32 subMapNo)
{
    SUB_MAP_C *pobjSubMap = SearchSubMap(subMapNo);
    if (pobjSubMap == NULL) {
        ST_LOG_ERR("Search No(%d) Fail.", subMapNo);
        return -1;
    }

    pobjWorkingSubMap_ = pobjSubMap;
    return 0;
}

SUB_MAP_C *COMB_MAP_C::SearchSubMap(UINT32 subMapNo)
{
    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        SUB_MAP_C *pobjSubMap = *iter;
        if (pobjSubMap->GetMapNo() == subMapNo) {
            return pobjSubMap;
        }
    }

    return NULL;
}

/**************************************************************************************
功能描述: 根据当前组合地图的位置，根据匹配对应的子地图，取离边距离最远的。
          判断小车位于哪一个子地图，如果有多个，则取离边远的
          返回为负表示无法匹配到子地图
修改记录:
**************************************************************************************/
SUB_MAP_C *COMB_MAP_C::MatchSubMapByUsingMinDist(const COOR_2D_POSE_STRU &stCombMapPose, int exceptSubMapNo)
{
    float maxDistSquToMapEdge = -1.0;
    SUB_MAP_C *pObjMaxDistSquSubMap = NULL;
    
    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        SUB_MAP_C *pobjSubMap = *iter;
        /* 被排除的子地图不处理，给定位置不在其中的子地图不处理 */
        if (INT32(pobjSubMap->GetMapNo()) == exceptSubMapNo) {
            continue;
        }
        
        if (!pobjSubMap->IsPointInMap(stCombMapPose.stPos)) {
            continue;
        }

        float distSquToMapEdge = pobjSubMap->GetMinDistSquToMapEdge(stCombMapPose.stPos);
        if (distSquToMapEdge < 0.0) {
            ST_LOG_ERR("Get Internal Point To Edge Dist Squ Fail.");
            continue;
        }

        if ((maxDistSquToMapEdge < 0.0) || (distSquToMapEdge > maxDistSquToMapEdge)) {
            maxDistSquToMapEdge  = distSquToMapEdge;
            pObjMaxDistSquSubMap = pobjSubMap;
        }
    }

    return pObjMaxDistSquSubMap;
}

/**************************************************************************************
功能描述: 根据当前组合地图的位置，根据匹配对应的子地图，取当前前进方向上离边距离最远的。
          判断小车位于哪一个子地图，如果有多个，则取离边远的
          返回为负表示无法匹配到子地图
修改记录:
**************************************************************************************/
SUB_MAP_C *COMB_MAP_C::MatchSubMapByUsingMoveDirDist(COOR_2D_POSE_STRU &stCombMapPose, int exceptSubMapNo, float &distSquToEdge)
{
    float maxDistSquToMapEdge = -1.0;
    SUB_MAP_C *pObjMaxDistSquSubMap = NULL;
    
    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        SUB_MAP_C *pobjSubMap = *iter;
        /* 被排除的子地图不处理，给定位置不在其中的子地图不处理 */
        if (INT32(pobjSubMap->GetMapNo()) == exceptSubMapNo) {
            continue;
        }
        
        if (!pobjSubMap->IsPointInMap(stCombMapPose.stPos)) {
            continue;
        }

        float distSquToMapEdge = pobjSubMap->GetDistSquToMapEdgeAlongMoveDir(stCombMapPose);
        if (distSquToMapEdge < 0.0) {
            ST_LOG_ERR("Get Internal Point To Edge Dist Squ Fail.");
            continue;
        }

        if ((maxDistSquToMapEdge < 0.0) || (distSquToMapEdge > maxDistSquToMapEdge)) {
            maxDistSquToMapEdge  = distSquToMapEdge;
            pObjMaxDistSquSubMap = pobjSubMap;
        }
    }

    distSquToEdge = maxDistSquToMapEdge;
    return pObjMaxDistSquSubMap;
}

/**************************************************************************************
功能描述: 从文件读取组合地图信息
修改记录:
**************************************************************************************/
int COMB_MAP_C::LoadFromFile(void)
{
    UINT32 minValidSubMapNo = 0;
    list<SUB_MAP_COMB_INFO_STRU> lstSubMapCombInfo;
    
    if (0 != objCombMapFile_.Read(lstSubMapCombInfo)) {
        ST_LOG_ERR("Read File Fail.");
        return -1;
    }
    
    /* 添加失败的时候要所有的都清除掉，防止异常信息残留 */
    for (list<SUB_MAP_COMB_INFO_STRU>::iterator iter = lstSubMapCombInfo.begin();
         iter != lstSubMapCombInfo.end();
         ++iter) {
        string strSubMapPath;
        objMapFileMng_.GetSubMapPath(iter->mapNo, strSubMapPath);

        GRAY_MAP_C            objMapFile;
        IMAGE_COORDINATE_STRU stMapSize;
        COORDINATE_2D_STRU    stOriginPosBySubMap;
        string                strMapPgmNameFrmYaml;
        if (0 != objMapFile.GetMapInfo(strSubMapPath, strMapPgmNameFrmYaml, stMapSize, stOriginPosBySubMap)) {
            ST_LOG_ERR("Get Map(%s) Info Fail.", strSubMapPath.c_str());
            Clear();
            return -1;
        }
        
        if (0 != AddSubMap(iter->mapNo, stMapSize, stOriginPosBySubMap, iter->stZeroPoseByCombMap)) {
            ST_LOG_ERR("Add List Fail.");
            Clear();
            return -1;
        }

        ST_LOG_INFO("Add Sub Map No(%u) Succ.", iter->mapNo);

        minValidSubMapNo = MAX(iter->mapNo + 1, minValidSubMapNo);
    }

    minValidSubMapNo_ = minValidSubMapNo;
    ST_LOG_INFO("Succ.");
    return 0;
}

/**************************************************************************************
功能描述: 将组合地图信息保存到文件
修改记录:
**************************************************************************************/
void COMB_MAP_C::SaveToFile(void)
{
    list<SUB_MAP_COMB_INFO_STRU> lstSubMapCombInfo;

    for (list<SUB_MAP_C *>::iterator iter = lstSubMap_.begin();
         iter != lstSubMap_.end();
         ++iter) {
        SUB_MAP_C *pobjSubMap = *iter;
        SUB_MAP_COMB_INFO_STRU stSubMapInfo;
        pobjSubMap->GetInfo(stSubMapInfo.mapNo, stSubMapInfo.stZeroPoseByCombMap);

        lstSubMapCombInfo.push_back(stSubMapInfo);
    }

    objCombMapFile_.Write(lstSubMapCombInfo);
}

/**************************************************************************************
功能描述:  合并子地图
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeSubMaps(list<UINT32> &lstSubMapNo, string &strMergedMapName)
{
    list<SUB_MAP_AREA_STRU> lstSubMapEffectArea;
    if (0 != GetSubMapsEffectArea(lstSubMapNo, lstSubMapEffectArea)) {
        ST_LOG_ERR("Get Area List Fail.");
        return -1;
    }
        
    COORDINATE_2D_STRU stMergedMapLeftButtomPoint;
    COORDINATE_2D_STRU stMergedMapRightTopPoint;
    if (0 != GetMergedMapArea(lstSubMapEffectArea, stMergedMapLeftButtomPoint, stMergedMapRightTopPoint)) {
        ST_LOG_ERR("Get Effect Area Fail.");
        return -1;
    }

    GRAY_MAP_C objMergedMap;
    if (0 != objMergedMap.CreateMap(stMergedMapLeftButtomPoint, stMergedMapRightTopPoint)) {
        ST_LOG_ERR("Create Map Fail.");
        return -1;
    }

    if (0 != MergeSubMapsArea(lstSubMapEffectArea, objMergedMap)) {
        ST_LOG_ERR("Copy Fail.");
        return -1;
    }

    string strMergedMapPath;
    objMapFileMng_.GetMapPath(strMergedMapName, strMergedMapPath);
    if (0 != objMergedMap.SaveMap(strMergedMapPath)) {
        ST_LOG_ERR("Save Map(%s) Fail.", strMergedMapPath.c_str());
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述:  合并子地图
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeSubMaps(UINT32 aSubMapNo[], int subMapNum, string &strMergedMapName)
{
    list<UINT32> lstSubMapNo;
    for (int loop = 0; loop < subMapNum; ++loop) {
        lstSubMapNo.push_back(aSubMapNo[loop]);
    }

    return MergeSubMaps(lstSubMapNo, strMergedMapName);
}

/**************************************************************************************
功能描述:  合并子地图，所有子地图
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeAllSubMaps(string &strMergedMapName)
{
    list<UINT32> lstSubMapNo;
    objCombMap_.GetSubMapList(lstSubMapNo);

    return MergeSubMaps(lstSubMapNo, strMergedMapName);
}

/**************************************************************************************
功能描述:  合并子地图，所有子地图，除了最后一个
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeAllSubMapsExceptLast(string &strMergedMapName)
{
    list<UINT32> lstSubMapNo;
    objCombMap_.GetSubMapList(lstSubMapNo);

    if (lstSubMapNo.size() > 1) {
        UINT32 lastSubMapNo = lstSubMapNo.back();
        ST_LOG_INFO("Except No = %u.", lastSubMapNo);
        lstSubMapNo.pop_back();
    }

    return MergeSubMaps(lstSubMapNo, strMergedMapName);
}

/**************************************************************************************
功能描述:  拆分子地图
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::SplitSubMap(GRAY_MAP_C &objMergedMap, SPLIT_SUB_MAP_INFO_STRU &stSplitSubMap)
{
    ST_LOG_INFO("Sub Map(%u).", stSplitSubMap.subMapNo);

    /* 先获取拆分的子地图的区域坐标 */
    IMAGE_COORDINATE_STRU stPictureLeftButtomCoord;
    stPictureLeftButtomCoord.x = stSplitSubMap.stPictureLeftTopCoord.x;
    stPictureLeftButtomCoord.y = stSplitSubMap.stPictureLeftTopCoord.y + stSplitSubMap.stImageSize.y - 1;
    COORDINATE_2D_STRU stLeftButtomPoint;
    if (0 != objMergedMap.TransPictureCoordToPos(stPictureLeftButtomCoord, stLeftButtomPoint)) {
        ST_LOG_ERR("Trans Left Buttom Point(%d, %d) Fail.", stSplitSubMap.stPictureLeftTopCoord.x, stSplitSubMap.stPictureLeftTopCoord.y);
        return -1;
    }

    IMAGE_COORDINATE_STRU stPictureRightTopCoord;
    stPictureRightTopCoord.x = stSplitSubMap.stPictureLeftTopCoord.x + stSplitSubMap.stImageSize.x - 1;
    stPictureRightTopCoord.y = stSplitSubMap.stPictureLeftTopCoord.y;
    COORDINATE_2D_STRU stRightTopPoint;
    if (0 != objMergedMap.TransPictureCoordToPos(stPictureRightTopCoord, stRightTopPoint)) {
        ST_LOG_ERR("Trans Right Top Point(%d, %d) Fail.", stPictureRightTopCoord.x, stPictureRightTopCoord.y);
        return -1;
    }

    /* 创建地图并复制内容和保存 */
    GRAY_MAP_C objSubMap;
    if (0 != objSubMap.CreateMap(stLeftButtomPoint, stRightTopPoint)) {
        ST_LOG_ERR("Create Map Fail.");
        return -1;
    }

    if (0 != objSubMap.Merge(stLeftButtomPoint, objMergedMap, stLeftButtomPoint, stRightTopPoint)) {
        ST_LOG_ERR("Copy To Sub Map Fail.");
        return -1;
    }

    string strSubMapName;
    objMapFileMng_.GetSubMapName(stSplitSubMap.subMapNo, strSubMapName);
    string strSubMapPath;
    objMapFileMng_.GetMapPath(strSubMapName, strSubMapPath);
    if (0 != objSubMap.SaveMap(strSubMapPath)) {
        ST_LOG_ERR("Save Map(%s) Fail.", strSubMapPath.c_str());
        return -1;
    }

    /* 组合地图中新建子地图 */
    COOR_2D_POSE_STRU stSubMapZeroPose;         /* 所有子地图的0点都时使用一样的，这样坐标也是一致的 */
    if (0 != objCombMap_.CreateSubMap(stSubMapZeroPose, strSubMapName)) {
        ST_LOG_ERR("Create Map Fail.");
        return -1;
    }

    ST_LOG_INFO("Create Sub Map(%u) Succ.", stSplitSubMap.subMapNo);
    return 0;
}

/**************************************************************************************
功能描述: 拆分子地图
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::SplitSubMaps(const string &strSrcMapName, list<SPLIT_SUB_MAP_INFO_STRU> &lstSplitSubMap)
{
    string strSrcMapPath;
    objMapFileMng_.GetMapPath(strSrcMapName, strSrcMapPath);
    
    GRAY_MAP_C objSrcMap;
    if (0 != objSrcMap.LoadMap(strSrcMapPath)) {
        ST_LOG_ERR("Load Src Map(%s) Fail.", strSrcMapPath.c_str());
        return -1;
    }
        
    for (list<SPLIT_SUB_MAP_INFO_STRU>::iterator iter = lstSplitSubMap.begin();
         iter != lstSplitSubMap.end();
         ++iter) {
        SPLIT_SUB_MAP_INFO_STRU &stSplitSubMap = *iter;
        if (0 != SplitSubMap(objSrcMap, stSplitSubMap)) {
            ST_LOG_ERR("Split Sub Map(%u) Fail.", stSplitSubMap.subMapNo);
            return -1;
        }
    }

    return 0;
}

/**************************************************************************************
功能描述: 拆分子地图，根据配置文件拆
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::SplitSubMaps(const string &strSrcMapName)
{
    ST_LOG_INFO("Start. Map Name = %s.", strSrcMapName.c_str());
    
    list<SPLIT_SUB_MAP_INFO_STRU> lstSplitSubMap;
    if (0 != GetSplitSubMaps(strSrcMapName, lstSplitSubMap)) {
        ST_LOG_ERR("Get Sub Map List Fail.");
        return -1;
    }

    return SplitSubMaps(strSrcMapName, lstSplitSubMap);
}

/**************************************************************************************
功能描述: 获取子地图在组合地图上的有效区域
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::GetSubMapEffectArea(UINT32 subMapNo, SUB_MAP_AREA_STRU &stSubMapEffectArea)
{
    /* 获取指定子地图有效区域 */
    string strSubMapName;
    objMapFileMng_.GetSubMapName(subMapNo, strSubMapName);
    string strSubMapPath;
    objMapFileMng_.GetMapPath(strSubMapName, strSubMapPath);
    
    GRAY_MAP_C objSubMap;
    if (0 != objSubMap.LoadMap(strSubMapPath)) {
        ST_LOG_ERR("Load Map Fail.");
        return -1;
    }

    COOR_2D_POSE_STRU stLeftButtomPointPoseBySubMap;
    COOR_2D_POSE_STRU stRightTopPointPoseBySubMap;
    if (0 != objSubMap.GetEffectArea(stLeftButtomPointPoseBySubMap.stPos, stRightTopPointPoseBySubMap.stPos)) {
        ST_LOG_ERR("Get Sub Map(%u) Effect Erea Fail.", subMapNo);
        return -1;
    }

    /* 将子地图区域转换为组合地图区域 */
    if (0 != objCombMap_.ChangeSubMap(strSubMapName)) {
        ST_LOG_ERR("Change Sub Map(%u) Fail.", subMapNo);
        return -1;
    }

    COOR_2D_POSE_STRU stLeftButtomPointPose;
    COOR_2D_POSE_STRU stRightTopPointPose;
    objCombMap_.TransSubToCombMapPose(stLeftButtomPointPoseBySubMap, stLeftButtomPointPose);
    objCombMap_.TransSubToCombMapPose(stRightTopPointPoseBySubMap, stRightTopPointPose);

    stSubMapEffectArea.subMapNo = subMapNo;
    stSubMapEffectArea.stLeftButtomPointBySubMap.Copy(stLeftButtomPointPoseBySubMap.stPos);
    stSubMapEffectArea.stRightTopPointBySubMap.Copy(stRightTopPointPoseBySubMap.stPos);
    stSubMapEffectArea.stLeftButtomPoint.Copy(stLeftButtomPointPose.stPos);
    stSubMapEffectArea.stRightTopPoint.Copy(stRightTopPointPose.stPos);

    ST_LOG_INFO("MapNo(%u), LB.x=%f:%f, LB.y=%f:%f, RT.x=%f:%f, RT.y=%f:%f.", 
            stSubMapEffectArea.subMapNo,
            stSubMapEffectArea.stLeftButtomPointBySubMap.x, stSubMapEffectArea.stLeftButtomPoint.x,
            stSubMapEffectArea.stLeftButtomPointBySubMap.y, stSubMapEffectArea.stLeftButtomPoint.y,
            stSubMapEffectArea.stRightTopPointBySubMap.x,   stSubMapEffectArea.stRightTopPoint.x,
            stSubMapEffectArea.stRightTopPointBySubMap.y,   stSubMapEffectArea.stRightTopPoint.y);
    return 0;
}

/**************************************************************************************
功能描述: 获取合并的子地图的总的有效区域
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::GetSubMapsEffectArea(list<UINT32> lstSubMapNo, list<SUB_MAP_AREA_STRU> &lstSubMapEffectArea)
{
    lstSubMapEffectArea.clear();
    
    for (list<UINT32>::iterator iter = lstSubMapNo.begin();
         iter != lstSubMapNo.end();
         ++iter) {
        SUB_MAP_AREA_STRU stSubMapEffectArea;
        if (0 != GetSubMapEffectArea(*iter, stSubMapEffectArea)) {
            ST_LOG_ERR("Get Sub Map(%u) Effect Area Fail.", *iter);
            return -1;
        }

        lstSubMapEffectArea.push_back(stSubMapEffectArea);
    }

    return 0;
}

/**************************************************************************************
功能描述: 获取合并的地图的信息
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::GetMergedMapArea(list<SUB_MAP_AREA_STRU> &lstSubMapArea, COORDINATE_2D_STRU &stLeftButtomPoint, COORDINATE_2D_STRU &stRightTopPoint)
{
    if (lstSubMapArea.size() <= 0) {
        return -1;
    }

    bool isSetFirst = true;
    
    for (list<SUB_MAP_AREA_STRU>::iterator iter = lstSubMapArea.begin();
         iter != lstSubMapArea.end();
         ++iter) {
        if (isSetFirst) {
            stLeftButtomPoint.Copy(iter->stLeftButtomPoint);
            stRightTopPoint.Copy(iter->stRightTopPoint);
            isSetFirst = false;
        } else {
            stLeftButtomPoint.x = MIN(stLeftButtomPoint.x, iter->stLeftButtomPoint.x);
            stLeftButtomPoint.y = MIN(stLeftButtomPoint.y, iter->stLeftButtomPoint.y);
            stRightTopPoint.x   = MAX(stRightTopPoint.x, iter->stRightTopPoint.x);
            stRightTopPoint.y   = MAX(stRightTopPoint.y, iter->stRightTopPoint.y);
        }
    }

    return 0;
}

/**************************************************************************************
功能描述: 将指定的子地图中的区域复制到合并地图中
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeSubMapArea(SUB_MAP_AREA_STRU &stSubMapArea, GRAY_MAP_C &objMergedMap)
{
    string strSubMapPath;
    objMapFileMng_.GetSubMapPath(stSubMapArea.subMapNo, strSubMapPath);
    
    GRAY_MAP_C objSubMap;
    if (0 != objSubMap.LoadMap(strSubMapPath)) {
        ST_LOG_ERR("Load Map Fail.");
        return -1;
    }

    if (0 != objMergedMap.Merge(stSubMapArea.stLeftButtomPoint, objSubMap, stSubMapArea.stLeftButtomPointBySubMap, stSubMapArea.stRightTopPointBySubMap)) {
        ST_LOG_ERR("Copy Fail.");
        return -1;
    }

    ST_LOG_INFO("Copy Map No(%u) Succ.", stSubMapArea.subMapNo);
    return 0;
}

/**************************************************************************************
功能描述: 将指定的子地图中的区域复制到合并地图中
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::MergeSubMapsArea(list<SUB_MAP_AREA_STRU> &lstSubMapArea, GRAY_MAP_C &objMergedMap)
{
    for (list<SUB_MAP_AREA_STRU>::iterator iter = lstSubMapArea.begin();
         iter != lstSubMapArea.end();
         ++iter) {
        if (0 != MergeSubMapArea(*iter, objMergedMap)) {
            ST_LOG_ERR("Get Sub Map(%u) Area Fail.", iter->subMapNo);
            return -1;
        }
    }

    objMergedMap.FilterObstOutlier(3, 2);
    return 0;
}

/**************************************************************************************
功能描述: 从配置文件中读取拆分子地图信息
修改记录:
**************************************************************************************/
int COMB_MAP_EDIT_C::GetSplitSubMaps(const string &strSrcMapName, list<SPLIT_SUB_MAP_INFO_STRU> &lstSplitSubMap)
{
    string strSplitCfgFilePath;
    objMapFileMng_.GetSplitCfgFilePath(strSrcMapName, strSplitCfgFilePath);
    
    TiXmlDocument objXmlFile(strSplitCfgFilePath.c_str());
    if (!objXmlFile.LoadFile()) {
        ST_LOG_ERR("Load File(%s) Fail.", strSplitCfgFilePath.c_str());
        return -1;
    }

    TiXmlElement *pobjCombMap = objXmlFile.RootElement();
    if (pobjCombMap == NULL) {
        return -1;
    }

    lstSplitSubMap.clear();

    for (TiXmlElement *pobjSubMap = pobjCombMap->FirstChildElement();
         pobjSubMap != NULL;
         pobjSubMap = pobjSubMap->NextSiblingElement()) {
        SPLIT_SUB_MAP_INFO_STRU stSplitSubMap;
        
        if (TIXML_SUCCESS != pobjSubMap->QueryUnsignedAttribute("No",     &stSplitSubMap.subMapNo)) {
            return -1;
        }

        if (TIXML_SUCCESS != pobjSubMap->QueryIntAttribute(   "LeftTopX", &stSplitSubMap.stPictureLeftTopCoord.x)) {
            return -1;
        }

        if (TIXML_SUCCESS != pobjSubMap->QueryIntAttribute(   "LeftTopY", &stSplitSubMap.stPictureLeftTopCoord.y)) {
            return -1;
        }
        
        if (TIXML_SUCCESS != pobjSubMap->QueryIntAttribute(   "SizeX",    &stSplitSubMap.stImageSize.x)) {
            return -1;
        }

        if (TIXML_SUCCESS != pobjSubMap->QueryIntAttribute(   "SizeY",    &stSplitSubMap.stImageSize.y)) {
            return -1;
        }

        ST_LOG_INFO("Sub Map(%u).", stSplitSubMap.subMapNo);
        lstSplitSubMap.push_back(stSplitSubMap);
    }

    ST_LOG_INFO("Get Succ. Total Num = %u.", UINT32(lstSplitSubMap.size()));
    return 0;
}


#endif


